//
// Created by ZhaoXiaoFei on 2022/7/28.
//

#ifndef VINS_ESTIMATOR_POSELOCALPARAMETERIZATION_HPP
#define VINS_ESTIMATOR_POSELOCALPARAMETERIZATION_HPP

#include "ceres/ceres.h"


class PoseLocalParameterization : public ceres::LocalParameterization{
public:

    virtual bool Plus(const double* x,
                      const double* delta,
                      double* x_plus_delta) const{
        Eigen::Map<const Eigen::Vector3d> T(x);
        Eigen::Map<const Eigen::Quaterniond> Q(x + 3);

        Eigen::Map<const Eigen::Vector3d> deltaT(delta);

        //只取前三项
        Eigen::Map<const Eigen::Vector3d> deltaQ(delta + 3);

        Eigen::Map<Eigen::Vector3d> resT(x_plus_delta);
        Eigen::Map<Eigen::Quaterniond> resQ(x_plus_delta + 3);

        resT = T + deltaT;
        resQ = (Q * Utility::deltaQ(deltaQ)).normalized();
        return true;
    }

    virtual bool ComputeJacobian(const double* x, double* jacobian) const{
        Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> resJacobian(jacobian);
        resJacobian.topRows<6>().setIdentity();
        resJacobian.bottomRows<1>().setZero();

        return true;
    }

    // Size of x.
    virtual int GlobalSize() const{
        return 7;
    }

    // Size of delta.
    virtual int LocalSize() const{
        return 6;
    }

};

#endif //VINS_ESTIMATOR_POSELOCALPARAMETERIZATION_HPP
